
#include "grid3d.h"
#include "vecs.h"
#include "g3dvec.h"
#include <stdlib.h>
#include <memory.h>

/****************************************************************************
 *                   The norm_scal_diff_x routines                          *
 ****************************************************************************/

double norm_scal_diff_l2_p1(const grid3d_t *grid, const scalar v1,
                            const scalar v2)
{
    int i, j;
    double S, **M, D[4];
    elem_t *T;

    use_scalar(v1)
    use_scalar(v2)
    S=0.0;
    for(T=grid->elems; T<grid->elems+grid->NEL; T++)
    {
        local_mass_p1(grid, T, &M);
        for(i=0; i<4; i++)
            D[i]=v1F[T->nodes[i]]-v2F[T->nodes[i]];
        for(i=0; i<4; i++) for(j=0; j<4; j++)
                S+=(D[i]*D[j])*M[i][j];
    }
    done_scalar(v2)
    done_scalar(v1)
    return sqrt(S);
}

double norm_scal_diff_h1_p1(const grid3d_t *grid, const scalar v1,
                            const scalar v2)
{
    int i, j;
    double S, **L, D[4];
    elem_t *T;

    use_scalar(v1)
    use_scalar(v2)
    S=0.0;
    for(T=grid->elems; T<grid->elems+grid->NEL; T++)
    {
        local_stif_p1(grid, T, &L);
        for(i=0; i<4; i++)
            D[i]=v1F[T->nodes[i]]-v2F[T->nodes[i]];
        for(i=0; i<4; i++) for(j=0; j<4; j++)
                S+=(D[i]*D[j])*L[i][j];
    }
    done_scalar(v2)
    done_scalar(v1)
    return sqrt(S);
}

double norm_scal_diff_l2_cr(const grid3d_t *grid, const scalar v1,
                            const scalar v2)
{
    int i, j;
    double S, **M, D[4];
    elem_t *T;

    use_scalar(v1)
    use_scalar(v2)
    S=0.0;
    for(T=grid->elems; T<grid->elems+grid->NEL; T++)
    {
        local_mass_cr(grid, T, &M);
        for(i=0; i<4; i++)
            D[i]=v1F[T->faces[i]]-v2F[T->faces[i]];
        for(i=0; i<4; i++) for(j=0; j<4; j++)
                S+=(D[i]*D[j])*M[i][j];
    }
    done_scalar(v2)
    done_scalar(v1)
    return sqrt(S);
}

double norm_scal_diff_h1_cr(const grid3d_t *grid, const scalar v1,
                            const scalar v2)
{
    int i, j;
    double S, **L, D[4];
    elem_t *T;

    use_scalar(v1)
    use_scalar(v2)
    S=0.0;
    for(T=grid->elems; T<grid->elems+grid->NEL; T++)
    {
        local_stif_cr(grid, T, &L);
        for(i=0; i<4; i++)
            D[i]=v1F[T->faces[i]]-v2F[T->faces[i]];
        for(i=0; i<4; i++) for(j=0; j<4; j++)
                S+=(D[i]*D[j])*L[i][j];
    }
    done_scalar(v2)
    done_scalar(v1)
    return sqrt(S);
}

/****************************************************************************/

void ConvertP1toCR(grid3d_t *grid, const vector P1, vector CR)
{
    int i, *n;
    face_t *f;
    assert((get_N_vec(P1)==grid->NN)&&(get_N_vec(CR)==grid->NF));
    use_vector(CR)
    use_vector(P1)
    for(i=0, f=grid->faces; i<grid->NF; i++, f++)
    {
        n=f->nodes;
        CRX[i]=(P1X[n[0]]+P1X[n[1]]+P1X[n[2]])/3.0;
        CRY[i]=(P1Y[n[0]]+P1Y[n[1]]+P1Y[n[2]])/3.0;
        CRZ[i]=(P1Z[n[0]]+P1Z[n[1]]+P1Z[n[2]])/3.0;
    }
    done_vector(P1)
    done_vector(CR)
}

/* ***** ********************************************************************/

void test_div_cr(const grid3d_t *grid, int test_bdry, const vector v, scalar p)
{
    int el, i, fn;
    double F, N[3];
    elem_t *T;

    assert(grid->NF==get_N_vec(v));
    assert(grid->NEL==get_N_scal(p));

    use_vector(v)
    use_scalar(p)

    clean_scal(p);
    for(el=0, T=grid->elems; el<grid->NEL; el++, T++)
    {
        for(i=0; i<4; i++)
        {
            fn=T->faces[i];
            if((!test_bdry)&&(fn>=grid->NFI)) continue;
            F=face_area(grid, grid->faces+fn, N);
            F*=elem_face_normal_sign(grid, T, i);
            pF[el]+=F*(vX[fn]*N[0]+vY[fn]*N[1]+vZ[fn]*N[2]);
        }
    }
    done_scalar(p)
    done_vector(v)
}

void test_grad_cr(const grid3d_t *grid, int test_bdry, const scalar p, vector v)
{
    int el, i, fn;
    double F, N[3];
    elem_t *T;

    assert(grid->NF==get_N_vec(v));
    assert(grid->NEL==get_N_scal(p));

    clean_vec(v);

    use_vector(v)
    use_scalar(p)
    for(el=0, T=grid->elems; el<grid->NEL; el++, T++)
    {
        for(i=0; i<4; i++)
        {
            fn=T->faces[i];
            if((!test_bdry)&&(fn>=grid->NFI)) continue;
            F=face_area(grid, grid->faces+fn, N);
            F*=elem_face_normal_sign(grid, T, i);
            vX[fn]+=pF[el]*F*N[0];
            vY[fn]+=pF[el]*F*N[1];
            vZ[fn]+=pF[el]*F*N[2];
        }
    }
    done_scalar(p)
    done_vector(v)
}

/* =========================================================================
        Conversions between x-y-z and t-s-n
   ========================================================================= */

void convert_xyz2tsn(const grid3d_t *grid, const vector xyz, vector tsn)
{
    double t[3], s[3], n[3], v[3], w[3];
    int i;

    assert(get_N_vec(xyz)==grid->NF);
    assert(get_N_vec(tsn)==grid->NF);

    use_vector(xyz)
    use_vector(tsn)
    for(i=0; i<grid->NF; i++)
    {
        face_vecs(grid, grid->faces+i, t, s, n);
        v[0]=xyzX[i];
        v[1]=xyzY[i];
        v[2]=xyzZ[i];
        /* this works only if t, s, and n are orthonormal (they are) */
        w[0]=mulvec(v, t);
        if(fabs(w[0])<SMALLF) w[0]=0.0;
        w[1]=mulvec(v, s);
        if(fabs(w[1])<SMALLF) w[1]=0.0;
        w[2]=mulvec(v, n);
        if(fabs(w[2])<SMALLF) w[2]=0.0;
        tsnX[i]=w[0];
        tsnY[i]=w[1];
        tsnZ[i]=w[2];
    }
    done_vector(tsn)
    done_vector(xyz)
}

void convert_tsn2xyz(const grid3d_t *grid, const vector tsn, vector xyz)
{
    double t[3], s[3], n[3], v[3], w[3];
    int i;

    assert(get_N_vec(xyz)==grid->NF);
    assert(get_N_vec(tsn)==grid->NF);

    use_vector(xyz)
    use_vector(tsn)
    for(i=0; i<grid->NF; i++)
    {
        face_vecs(grid, grid->faces+i, t, s, n);
        v[0]=tsnX[i];
        v[1]=tsnY[i];
        v[2]=tsnZ[i];
        w[0]=v[0]*t[0]+v[1]*s[0]+v[2]*n[0];
        w[1]=v[0]*t[1]+v[1]*s[1]+v[2]*n[1];
        w[2]=v[0]*t[2]+v[1]*s[2]+v[2]*n[2];
        if(fabs(w[0])<SMALLF) w[0]=0.0;
        if(fabs(w[1])<SMALLF) w[1]=0.0;
        if(fabs(w[2])<SMALLF) w[2]=0.0;
        xyzX[i]=w[0];
        xyzY[i]=w[1];
        xyzZ[i]=w[2];
    }
    done_vector(tsn)
    done_vector(xyz)
}

void convert_xyz2tsdf(const grid3d_t *grid, const vector xyz, double *tsdf)
{
    int fid, eid, i;
    static double v[3], t[3], s[3], n[3];
    double vn, c, *T, *S, *DF;
    face_t *f;

    assert(get_N_vec(xyz)==grid->NF);

    T=tsdf;
    S=tsdf+grid->NF;
    DF=tsdf+2*grid->NF;
    memset(tsdf, 0, sizeof(double) *(2*grid->NF+grid->NE));

    use_vector(xyz)
    for(fid=0; fid<grid->NF; fid++)
    {
        f=grid->faces+fid;
        face_vecs(grid, f, t, s, n);
        v[0]=xyzX[fid];
        v[1]=xyzY[fid];
        v[2]=xyzZ[fid];
        T[fid]=mulvec(v, t);
        S[fid]=mulvec(v, s);
        vn=mulvec(v, n);
        for(i=0; i<3; i++)
        {
            eid=f->edges[i];
            c=edge_face_coef_vect(grid, grid->edges+eid, f, NULL);
            DF[eid]+=c * vn;
        }
    }
    done_vector(xyz)

    for(i=0; i<2*grid->NF+grid->NE; i++)
        if(fabs(T[i])<SMALLF) T[i]=0.0;
    return;
}

/****************************************************************************/
/*
void AssembleGravity(grid3d_t *grid, vector G)
{
    int i, j;
    double F, R, V;
    elem_t *T;
    signed S;

    assert(get_N_vec(G)==grid->NF);
    double *GZ = get_Z_vec(G);    
    clean_vec(G);
    F = GetFr();
    if(F <=SMALLF) return;
    S = 1;
    for(i=0;i<grid->NEL;i++)
    {
        T = grid->elems+i;
        V = elem_volume(grid,T);
        R = GetRho(T);
        for(j=0;j<4;j++)
            GZ[T->faces[j]] -= S * R / F * V * 0.25;
    }
}
 */
